
/*
    Revolution Engine
    Copyright (C) 2009 Felipe Apablaza

    This library is free software; you can redistribute it and/or
    modify it under the terms of the GNU Lesser General Public
    License as published by the Free Software Foundation; either
    version 2.1 of the License, or (at your option) any later version.

    This library is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
    Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public
    License along with this library; if not, write to the Free Software
    Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
*/


/**
 *
 *  File: Matrix4.hpp
 *  Desc: Four-Order matrix class template (OpenGL-Compliant)
 *
 */

#ifndef __REVENG_MATH_MATRIX4_HPP__
#define __REVENG_MATH_MATRIX4_HPP__

namespace RevEng
{
    namespace Math
    {
		template<typename T>
		struct Matrix4
		{
            T M11, M21, M31, M41;
            T M12, M22, M32, M42;
            T M13, M23, M33, M43;
            T M14, M24, M34, M44;
			
			Matrix4<T>();
			Matrix4<T>(const T* data);
			Matrix4<T>( T m11, T m12, T m13, T m14,
                        T m21, T m22, T m23, T m24,
                        T m31, T m32, T m33, T m34,
                        T m41, T m42, T m43, T m44);
			Matrix4<T>(const Matrix4<T> &m);
			
			Matrix4<T>& Inverse(T Det);
			Matrix4<T>& Inverse();
			Matrix4<T>& Transpose();
			
			T GetDeterminant() const;

			Matrix4<T> &operator= (const Matrix4<T> &M);

			bool operator == (const Matrix4<T> &M) const;
			bool operator != (const Matrix4<T> &M) const;

			Matrix4<T> operator+ (const Matrix4<T> &M)const;
			Matrix4<T> operator- (const Matrix4<T> &M)const;
			Matrix4<T> operator- () const;
			Matrix4<T> operator* (const Matrix4<T> &M)const;
			Matrix4<T> operator* (const T &S)const;
			Matrix4<T> operator/ (const T &S)const;
			
			Matrix4<T>& operator+= (const Matrix4<T> &M);
			Matrix4<T>& operator-= (const Matrix4<T> &M);
			Matrix4<T>& operator*= (const Matrix4<T> &M);
			Matrix4<T>& operator*= (const T &S);
			Matrix4<T>& operator/= (const T &S);
			
			friend Matrix4<T> operator* (T S, const Matrix4<T> &M);
			
			operator T* ();
			operator T* () const;
			operator const T* ();
			operator const T* () const;
		};
		

		template<typename T>
		Matrix4<T>::Matrix4() :
			M11(0.0f), M21(0.0f), M31(0.0f), M41(0.0f),
            M12(0.0f), M22(0.0f), M32(0.0f), M42(0.0f),
            M13(0.0f), M23(0.0f), M33(0.0f), M43(0.0f),
            M14(0.0f), M24(0.0f), M34(0.0f), M44(0.0f)
		{
		}
		
        template<typename T> 
		Matrix4<T>::Matrix4(    T m11, T m12, T m13, T m14,
                                T m21, T m22, T m23, T m24,
                                T m31, T m32, T m33, T m34,
                                T m41, T m42, T m43, T m44)	
            :
                M11(m11), M21(m21), M31(m31), M41(m41),
                M12(m12), M22(m22), M32(m32), M42(m42),
                M13(m13), M23(m23), M33(m33), M43(m43),
                M14(m14), M24(m24), M34(m34), M44(m44)
		{
		}


		template<typename T> 
		Matrix4<T>::Matrix4(const Matrix4<T> &m)
		{
			memcpy(*this, m, sizeof(T)*16);
		}
		

		template<typename T> 
		Matrix4<T>& Matrix4<T>::Inverse()
		{
		    Inverse( GetDeterminant() );

			return *this;
		}


		template<typename T> 
		Matrix4<T>& Matrix4<T>::Inverse(T Det)
		{
		    		    /*
                m11 m12 m13 m14
                m21 m22 m23 m24
                m31 m32 m33 m34
                m41 m42 m43 m44
		    */
		    
		    Matrix3<T> a11(     M22, M23, M24,
                                M32, M33, M34,
                                M42, M43, M44);
                                
            Matrix3<T> a12(     M21, M23, M24,
                                M31, M33, M34,
                                M41, M43, M44);
                                
            Matrix3<T> a13(     M21, M22, M24,
                                M31, M32, M34,
                                M41, M42, M44);

            Matrix3<T> a14(     M21, M22, M23,
                                M31, M32, M33,
                                M41, M42, M43);

		    /*
                m11 m12 m13 m14
                m21 m22 m23 m24
                m31 m32 m33 m34
                m41 m42 m43 m44
		    */
		    
		    Matrix3<T> a21(     M12, M13, M14,
                                M32, M33, M34,
                                M42, M43, M44);
                                
            Matrix3<T> a22(     M11, M13, M14,
                                M31, M33, M34,
                                M41, M43, M44);
                                
            Matrix3<T> a23(     M11, M12, M14,
                                M31, M32, M34,
                                M41, M42, M44);
                                
            Matrix3<T> a24(     M11, M12, M13,
                                M31, M32, M33,
                                M41, M42, M43);

		    /*
                m11 m12 m13 m14
                m21 m22 m23 m24
                m31 m32 m33 m34
                m41 m42 m43 m44
		    */
            Matrix3<T> a31(     M12, M13, M14,
                                M22, M23, M24,
                                M42, M43, M44);

            Matrix3<T> a32(     M11, M13, M14,
                                M21, M23, M24,
                                M41, M43, M44);
                                
            Matrix3<T> a33(     M11, M12, M14,
                                M21, M22, M24,
                                M41, M42, M44);
                                
            Matrix3<T> a34(     M11, M12, M13,
                                M21, M22, M23,
                                M41, M42, M43);

		    /*
                m11 m12 m13 m14
                m21 m22 m23 m24
                m31 m32 m33 m34
                m41 m42 m43 m44
		    */
		    
            Matrix3<T> a41(     M12, M13, M14,
                                M22, M23, M24,
                                M32, M33, M34);
            
            Matrix3<T> a42(     M11, M13, M14,
                                M21, M23, M24,
                                M31, M33, M34);
                                
            Matrix3<T> a43(     M11, M12, M14,
                                M21, M22, M24,
                                M31, M32, M34);

            Matrix3<T> a44(     M11, M12, M13,
                                M21, M22, M23,
                                M31, M32, M33);
            
            M11 = M11 * a11.GetDeterminant();
            M21 = M21 * a21.GetDeterminant();
            M31 = M31 * a31.GetDeterminant();
            M41 = M41 * a41.GetDeterminant();
            
            M12 = M12 * a12.GetDeterminant();
            M22 = M22 * a22.GetDeterminant();
            M32 = M32 * a32.GetDeterminant();
            M42 = M42 * a42.GetDeterminant();
            
            M13 = M13 * a13.GetDeterminant();
            M23 = M23 * a23.GetDeterminant();
            M33 = M33 * a33.GetDeterminant();
            M43 = M43 * a43.GetDeterminant();
            
            M14 = M14 * a14.GetDeterminant();
            M24 = M24 * a24.GetDeterminant();
            M34 = M34 * a34.GetDeterminant();
            M44 = M44 * a44.GetDeterminant();
            
            Transpose();
            
            *this /= Det;
            
			return *this;
		}


		template<typename T> 
		Matrix4<T>& Matrix4<T>::Transpose()
		{
		    std::swap(M12, M21);
		    std::swap(M13, M31);
		    std::swap(M14, M41);
		    
		    std::swap(M24, M42);
		    std::swap(M34, M43);
		    
		    std::swap(M23, M32);
		    
			return *this;
		}


		template<typename T> 
		T Matrix4<T>::GetDeterminant() const
		{
			//Pre-calcular determinantes de los cofactores de la matriz
			T   D31_32 = M31*M42 - M41*M32,
				D31_33 = M31*M43 - M41*M33,
				D31_34 = M31*M44 - M41*M34,
				D32_33 = M32*M43 - M42*M33,
				D32_34 = M32*M44 - M42*M34,
				D33_34 = M33*M44 - M43*M34;

			//Calcular la determinante de la matriz
			return  M11*(M22*D33_34 - M23*D32_34 + M24*D32_33) -
					M12*(M21*D33_34 - M23*D31_34 + M24*D31_33) +
					M13*(M21*D32_34 - M22*D31_34 + M24*D31_32) -
					M14*(M21*D32_33 - M22*D31_33 + M23*D31_32);
		}


		template<typename T> 
		Matrix4<T> &Matrix4<T>::operator= (const Matrix4<T> &M)
		{
			::memcpy(*this, M, sizeof(T)*16);
			return *this;
		}


		template<typename T> 
		Matrix4<T> Matrix4<T>::operator+ (const Matrix4<T> &M)const
		{
			return Matrix4<T>
			(
				M11+M.M11, M12+M.M12, M13+M.M13, M14+M.M14,
				M21+M.M21, M22+M.M22, M23+M.M23, M24+M.M24,
				M31+M.M31, M32+M.M32, M33+M.M33, M34+M.M34,
				M41+M.M41, M42+M.M42, M43+M.M43, M44+M.M44
			);
		}


		template<typename T> 
		Matrix4<T> Matrix4<T>::operator- (const Matrix4<T> &M)const
		{
			return Matrix4<T>
			(
				M11-M.M11, M12-M.M12, M13-M.M13, M14-M.M14,
				M21-M.M21, M22-M.M22, M23-M.M23, M24-M.M24,
				M31-M.M31, M32-M.M32, M33-M.M33, M34-M.M34,
				M41-M.M41, M42-M.M42, M43-M.M43, M44-M.M44
			);
		}


		template<typename T> 
		Matrix4<T> Matrix4<T>::operator- () const
		{
			return Matrix4<T>
			(
				-M11, -M12, -M13, -M14,
				-M21, -M22, -M23, -M24,
				-M31, -M32, -M33, -M34,
				-M41, -M42, -M43, -M44
			);
		}


		template<typename T> 
		Matrix4<T> Matrix4<T>::operator* (const Matrix4<T> &M)const
		{
			return Matrix4<T>
			(
				M11*M.M11 + M12*M.M21 + M13*M.M31 + M14*M.M41,
				M11*M.M12 + M12*M.M22 + M13*M.M32 + M14*M.M42,
				M11*M.M13 + M12*M.M23 + M13*M.M33 + M14*M.M43,
				M11*M.M14 + M12*M.M24 + M13*M.M34 + M14*M.M44,

				M21*M.M11 + M22*M.M21 + M23*M.M31 + M24*M.M41,
				M21*M.M12 + M22*M.M22 + M23*M.M32 + M24*M.M42,
				M21*M.M13 + M22*M.M23 + M23*M.M33 + M24*M.M43,
				M21*M.M14 + M22*M.M24 + M23*M.M34 + M24*M.M44,

				M31*M.M11 + M32*M.M21 + M33*M.M31 + M34*M.M41,
				M31*M.M12 + M32*M.M22 + M33*M.M32 + M34*M.M42,
				M31*M.M13 + M32*M.M23 + M33*M.M33 + M34*M.M43,
				M31*M.M14 + M32*M.M24 + M33*M.M34 + M34*M.M44,

				M41*M.M11 + M42*M.M21 + M43*M.M31 + M44*M.M41,
				M41*M.M12 + M42*M.M22 + M43*M.M32 + M44*M.M42,
				M41*M.M13 + M42*M.M23 + M43*M.M33 + M44*M.M43,
				M41*M.M14 + M42*M.M24 + M43*M.M34 + M44*M.M44
			);
		}


		template<typename T> 
		Matrix4<T> Matrix4<T>::operator* (const T &S)const
		{
			return Matrix4<T>
			(
				M11*S, M12*S, M13*S, M14*S,
				M21*S, M22*S, M23*S, M24*S,
				M31*S, M32*S, M33*S, M34*S,
				M41*S, M42*S, M43*S, M44*S
			);
		}


		template<typename T> 
		Matrix4<T> Matrix4<T>::operator/ (const T &S)const
		{
			return Matrix4<T>
			(
				M11/S, M12/S, M13/S, M14/S,
				M21/S, M22/S, M23/S, M24/S,
				M31/S, M32/S, M33/S, M34/S,
				M41/S, M42/S, M43/S, M44/S
			);
		}

		template<typename T> 
		Matrix4<T>& Matrix4<T>::operator+= (const Matrix4<T> &M)
		{
		    M11 += M.M11;
		    M21 += M.M21;
		    M31 += M.M31;
		    M41 += M.M41;
		    
		    M12 += M.M12;
		    M22 += M.M22;
		    M32 += M.M32;
		    M42 += M.M42;
		    
		    M13 += M.M13;
		    M23 += M.M23;
		    M33 += M.M33;
		    M43 += M.M43;
		    
		    M14 += M.M14;
		    M24 += M.M24;
		    M34 += M.M34;
		    M44 += M.M44;
			
			return *this;
		}


		template<typename T> 
		Matrix4<T>& Matrix4<T>::operator-= (const Matrix4<T> &M)
		{
            M11 -= M.M11;
		    M21 -= M.M21;
		    M31 -= M.M31;
		    M41 -= M.M41;
		    
		    M12 -= M.M12;
		    M22 -= M.M22;
		    M32 -= M.M32;
		    M42 -= M.M42;
		    
		    M13 -= M.M13;
		    M23 -= M.M23;
		    M33 -= M.M33;
		    M43 -= M.M43;
		    
		    M14 -= M.M14;
		    M24 -= M.M24;
		    M34 -= M.M34;
		    M44 -= M.M44;
		    
			return *this;
		}


		template<typename T> 
		Matrix4<T>& Matrix4<T>::operator*= (const Matrix4<T> &M)
		{
			Matrix4<T> Rest;

			Rest.M11 = M11*M.M11 + M12*M.M21 + M13*M.M31 + M14*M.M41;
            Rest.M12 = M11*M.M12 + M12*M.M22 + M13*M.M32 + M14*M.M42;
			Rest.M13 = M11*M.M13 + M12*M.M23 + M13*M.M33 + M14*M.M43;
			Rest.M14 = M11*M.M14 + M12*M.M24 + M13*M.M34 + M14*M.M44;

			Rest.M21 = M21*M.M11 + M22*M.M21 + M23*M.M31 + M24*M.M41;
			Rest.M22 = M21*M.M12 + M22*M.M22 + M23*M.M32 + M24*M.M42;
			Rest.M23 = M21*M.M13 + M22*M.M23 + M23*M.M33 + M24*M.M43;
			Rest.M24 = M21*M.M14 + M22*M.M24 + M23*M.M34 + M24*M.M44;

			Rest.M31 = M31*M.M11 + M32*M.M21 + M33*M.M31 + M34*M.M41;
			Rest.M32 = M31*M.M12 + M32*M.M22 + M33*M.M32 + M34*M.M42;
			Rest.M33 = M31*M.M13 + M32*M.M23 + M33*M.M33 + M34*M.M43;
			Rest.M34 = M31*M.M14 + M32*M.M24 + M33*M.M34 + M34*M.M44;

			Rest.M41 = M41*M.M11 + M42*M.M21 + M43*M.M31 + M44*M.M41;
			Rest.M42 = M41*M.M12 + M42*M.M22 + M43*M.M32 + M44*M.M42;
			Rest.M43 = M41*M.M13 + M42*M.M23 + M43*M.M33 + M44*M.M43;
			Rest.M44 = M41*M.M14 + M42*M.M24 + M43*M.M34 + M44*M.M44;

			return *this = Mat;
		}


		template<typename T> 
		Matrix4<T>& Matrix4<T>::operator*= (const T &S)
		{
			M11 *= S;
		    M21 *= S;
		    M31 *= S;
		    M41 *= S;
		    
		    M12 *= S;
		    M22 *= S;
		    M32 *= S;
		    M42 *= S;
		    
		    M13 *= S;
		    M23 *= S;
		    M33 *= S;
		    M43 *= S;
		    
		    M14 *= S;
		    M24 *= S;
		    M34 *= S;
		    M44 *= S;

			return *this;
		}


		template<typename T> 
		Matrix4<T>& Matrix4<T>::operator/= (const T &S)
		{
		    T InvS = static_cast<T>(1) / S;
		    
			M11 *= InvS;
		    M21 *= InvS;
		    M31 *= InvS;
		    M41 *= InvS;
		    
		    M12 *= InvS;
		    M22 *= InvS;
		    M32 *= InvS;
		    M42 *= InvS;
		    
		    M13 *= InvS;
		    M23 *= InvS;
		    M33 *= InvS;
		    M43 *= InvS;
		    
		    M14 *= InvS;
		    M24 *= InvS;
		    M34 *= InvS;
		    M44 *= InvS;

			return *this;
		}


		template<typename T> 
		Matrix4<T> operator* (T S, const Matrix4<T> &M)
		{
			return M*S;
		}


		template<typename T> 
		Matrix4<T>::operator T* ()
		{
			return &M11;
		}		
		
		
		template<typename T> 
		Matrix4<T>::operator T* () const
		{
			return &M11;
		}		
		
		
		template<typename T> 
		Matrix4<T>::operator const T* ()
		{
			return &M11;
		}		
		
		
		template<typename T> 
		Matrix4<T>::operator const T* () const
		{
			return &M11;
		}		
    }
}
#endif // __REVENG_MATH_MATRIX4_HPP__
